//#include <Servo.h>
#include <FlexiTimer2.h>


int motor_speed_left = 90;
int motor_speed_right = 90;

/*
int motor_speed_old_left = 90;
int motor_speed_old_right = 90;
*/

int REMOTE_MAX = 1900;  //遥控器最小和最大的数
int REMOTE_MIN = 1000;

int MIN_MOTOR = 70;
int MAX_MOTOR = 110;

int durgodown = 0; 
int durleftright = 0;
int durGear = 0;  //手动或者自动 持续时间

const int godownPin = 4, leftrightPin = 5, gearPin = 6;  //前进后退，转弯，自动手动开关信号
boolean autoFlag = 1;
int count = 0;

struct  //total 2+14+2=18 bytes
{
  //header (total 2 bytes)
  byte header1;  // 1 bytes
  byte header2;  // 1 bytes

  //control part (total 10 bytes)
  int autoFlag;    // 2 //rc_ctrl
  int motorSpeedleft;  // 2 bytes  //current motor
  int motorSpeedright;  // 2 bytes  //current motor


  //crc (total 2 bytes)
  unsigned int crcnum;  //2

} arduinoData = {0};

void structDataSend() {
  // 存疑，这里是否还需要映射一次？
  int tmp_motor_left = map(motor_speed_left, MIN_MOTOR, MAX_MOTOR, 0, 100);
  int tmp_motor_right = map(motor_speed_right, MIN_MOTOR, MAX_MOTOR, 0, 100);
  arduinoData.autoFlag = autoFlag;
  arduinoData.motorSpeedleft = tmp_motor_left;
  arduinoData.motorSpeedright = tmp_motor_right;
  // 加一个校验数，确保数据的完整性
  byte *tobyte = (byte *)&arduinoData;
  arduinoData.crcnum = CRC16(tobyte + 2, sizeof(arduinoData) - 4);  //the valid data part as used to generate crc
  //  Serial.println(sizeof(sensorData));
  // 发送到主板 USB 串口
  Serial.write(tobyte, sizeof(arduinoData));
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(godownPin, INPUT);
  pinMode(leftrightPin, INPUT);
  pinMode(gearPin, INPUT);
  
  
  arduinoData.header1 = 0x4f;
  arduinoData.header2 = 0x5e;

  FlexiTimer2::set(100, flash);
  FlexiTimer2::start();
}



// 写数据时用到
unsigned int CRC16(const byte *pBuffer, unsigned int bufferSize) {
  unsigned int poly = 0x8408;
  unsigned int crc = 0;
  byte carry;
  byte i_bits;
  unsigned int j;
  for (j = 0; j < bufferSize; j++) {
    crc = crc ^ pBuffer[j];
    for (i_bits = 0; i_bits < 8; i_bits++) {
      carry = crc & 1;
      crc = crc / 2;
      if (carry) {
        crc = crc ^ poly;
      }
    }
  }
  return crc;
}


void signalSelection() {
  durgodown = pulseIn(godownPin, HIGH);
  int godown = map(durgodown, REMOTE_MIN, REMOTE_MAX, MIN_MOTOR, MAX_MOTOR);

  durleftright = pulseIn(leftrightPin, HIGH);
  int leftright = map(durleftright, REMOTE_MIN, REMOTE_MAX, MIN_MOTOR, MAX_MOTOR);
  // 存疑 leftright本身可能就有正负号，应该这样写：
  // motor_speed_right = godown - leftright;
  // motor_speed_left = godown + leftright;
  if(durleftright < (REMOTE_MIN + REMOTE_MAX)/2){
    motor_speed_right = godown + leftright;
    motor_speed_left = godown - leftright;
  }
  else {
    motor_speed_right = godown - leftright;
    motor_speed_left = godown + leftright;
  }
}



void veloLimit() {
  // 存疑 这里需要根据实际情况修改限制速度
  motor_speed_left = constrain(motor_speed_left, 0, 180);
  // if (motor_speed > 80 and motor_speed < 100){
  //     motor_speed = 90;
  // }
  motor_speed_right = constrain(motor_speed_right, 0, 180);
}



//防止误触
/*
void WriteData() {

  if (abs(motor_speed - motor_speed_old) > 4) {
    motor.write(motor_speed);
    motor_speed_old = motor_speed;
  }
  if (rudder_pos >= -40 && rudder_pos <= 40 && abs(rudder_pos - rudder_pos_old) > 3) {
    //rudder.write(rudder_pos);
    rudder_pos_old = rudder_pos;
  }
  if (sail_pos >= 0 && sail_pos <= 90) {
    //sail.write(sail_pos);
    sail_pos_old = sail_pos;
  }
}
*/

void flash() {
  count++;
  if (count == 10) {  //read the gearPin every 10 intervals
    durGear = pulseIn(gearPin, HIGH, 20000); //count = 10 每隔1s检测一次 
    count = 0;
  }

  if (durGear > 1500 && durGear < 2500) {
    autoFlag = 1;
  }
  else {
    autoFlag = 0;
  }

  signalSelection();      // 帆舵 电机-手动控制
  veloLimit();
  //WriteData();
  structDataSend();
}


void loop() {
  // put your main code here, to run repeatedly:
}